#pragma once
#include <cmath>
#include <Eigen/Eigen>
#include<fstream>
#include <iostream>
using namespace std;
using namespace Eigen;
#define iszero( x)        (fabs(x)<em?1:0)
#define em 1e-6
void Pose2Matrix(double pose[]);